/**
  ******************************************************************************
  * @file    PID.c
  * @author  STMicroelectronics
  * @date    31-May-2022
  * @brief   PI Module
  ******************************************************************************
  *
  * @copyright (c) 2018(-2024) STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file in
  * the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  ******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "DPC_Pid.h"

/* Private variables ---------------------------------------------------------*/
// PBo - Variables should be moved into the header file without initialization
// PID global variables: can be tuned via debugger
float pid_Kp = 75;               /* Proportional term scaling */
float pid_Ki = 5;                /* Integral term scaling */
float pid_Kd = 0;                /* Differential term scaling */
float pid_Ks = 0;                /* Anti windup scaling */
float pid_int_term = 0;          /* Integral term */
float prev_pid_antiw_term = 0;   /* Anti windup term */
float err;                       /* Reference - feedback error */
float d_err;                     /* Error derivative */
float pid_output;                /* Output from PID computation  */
float pid_output_sat;            /* Output with saturation applied */
float pid_const_term = 0;        /* Constant term */


/* Private typedef -----------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

/**
* @brief  DPC_PI_Init: Init PI Data Struct.
* @param  pPI: pointer to a DPC_PI_t  that contains
*         the configuration information and data for the specified PI. 
*
* @param  Init_Val_Kp: Init Kp data
* @param  Init_Val_Ki: Init Ki data
* @param  Init_Val_Ts: Init Ts data
* @param  Init_PIsat_up: Init PI saturation Up data
* @param  Init_PIsat_down: Init PI saturation Down data
* @param  resetValue: FeedForward term of PLL (expressed in Hz i.e. 50Hz or 60Hz)
* 
* @retval None
*
* @note Function valid for STM32G4xx microcontroller family  
*/  
void DPC_PI_Init(DPC_PI_t *pPI,float Init_Val_Kp,float Init_Val_Ki,float Init_Val_Ts,float Init_PIsat_up, float Init_PIsat_down,FlagStatus satPI_toggle_local,FlagStatus antiwindPI_toggle_local,float Antiwindup_Gain_local,float resetValue)
{
  pPI->Kp = Init_Val_Kp;
  pPI->Ki = Init_Val_Ki;
  pPI->Ts = Init_Val_Ts;
  pPI->Integral = 0;
  pPI->PIout = 0;
  pPI->PIsat_up = Init_PIsat_up;
  pPI->PIsat_down = Init_PIsat_down;
  pPI->error = 0;
  pPI->Integralout = 0;
  pPI->resetPI = RESET;
  pPI->k0 = Init_Val_Kp; //K0=Kp
  pPI->k1 = (Init_Val_Ki * Init_Val_Ts); //K1=Ki*Ts
  pPI->satPI_toggle = satPI_toggle_local;
  pPI->antiwindPI_toggle = antiwindPI_toggle_local;
  pPI->Antiwindup_Gain = Antiwindup_Gain_local;
  pPI->resetValue = resetValue;
  
  pPI->PI_Q_FxP = 16;
  pPI->PIsat_up_s32 = (int32_t)(Init_PIsat_up * (1 << pPI->PI_Q_FxP));
  pPI->PIsat_down_s32 = (int32_t)(Init_PIsat_down * (1 << pPI->PI_Q_FxP));
  
}

/**
* @brief  DPC_PI: PI function
* @param  Ref: 
* @param  Feed: 
* @param  pPI: pointer to a DPC_PI_t  that contains
*         the configuration information and data for the specified PI. 
*
* @retval float Return output data of PI regulator
*
* @note Function valid for STM32G4xx microcontroller family  
*/
float DPC_PI(float Ref, float Feed , DPC_PI_t *pPI)
{
  pPI->Ref = Ref;
  pPI->Feed = Feed;
  
  pPI->error = (float)Ref - (float)Feed;
  
  if (pPI->resetPI == SET)
  {
    pPI->Integral = pPI->resetValue;
  }
  else
  {
    pPI->Integral = (pPI->Integral + (pPI->k1 * pPI->error) + pPI->Antiwindup_Term);
  }
  pPI->Integralout = pPI->Integral;
  pPI->PIout = ((pPI->k0 * pPI->error) + pPI->Integralout);

  //Start Check Saturation
  if (pPI->satPI_toggle == SET)
  {
    //Saturation
    if (pPI->PIout > pPI->PIsat_up)
    {
      pPI->PIout_sat = pPI->PIsat_up;
    }
    else if (pPI->PIout < pPI->PIsat_down)
    {
      pPI->PIout_sat = pPI->PIsat_down;
    }
    else
	{
      pPI->PIout_sat = pPI->PIout;
    }
    
    //Start Check Antiwindup
    if (pPI->antiwindPI_toggle == SET)
	{
      //Saturation
      pPI->Antiwindup_Term = ((pPI->PIout_sat - pPI->PIout) * pPI->Antiwindup_Gain);
    }
    else
	{
      pPI->Antiwindup_Term = 0;
    }
    //End Check Antiwindup    
  }
  else
  {
    pPI->PIout_sat = pPI->PIout;  
    pPI->Antiwindup_Term = 0;
  }
  //End Check Saturation
  
  return pPI->PIout_sat;  
}



/**
* @brief  DPC_PI_s32: PI function
* @param  Ref: 
* @param  Feed: 
* @param  pPI: pointer to a DPC_PI_t  that contains
*         the configuration information and data for the specified PI. 
*
* @retval float Return output data of PI regulator
*
* @note Function valid for STM32G4xx microcontroller family  
*/
int32_t DPC_PI_s32(int32_t Ref_s32, int32_t Feed_s32 , DPC_PI_t *pPI)
{
  pPI->Ref_s32 = Ref_s32;
  pPI->Feed_s32 = Feed_s32;
  
  pPI->error_s32 = Ref_s32-Feed_s32;
  
  if (pPI->resetPI == SET)
  {
    pPI->Integral_s32 = pPI->resetValue_s32;
  }
  else
  {
    pPI->Integral_s32 = (pPI->Integral_s32 + (pPI->k1_s32 * pPI->error_s32) + pPI->Antiwindup_Term_s32);
  }
  pPI->Integralout_s32 = pPI->Integral_s32;
  pPI->PIout_s32 = ((pPI->k0_s32 * pPI->error_s32) + pPI->Integralout_s32);
  
  //Start Check Saturation
  if (pPI->satPI_toggle == SET)
  {
    //Saturation
    if (pPI->PIout_s32 > pPI->PIsat_up_s32)
    {
      pPI->PIout_sat_s32 = pPI->PIsat_up_s32;
    }
    else if (pPI->PIout_s32 < pPI->PIsat_down_s32)
    {
      pPI->PIout_sat_s32 = pPI->PIsat_down_s32;
    }
    else
	{
      pPI->PIout_sat_s32 = pPI->PIout_s32;
    }
    
    //Start Check Antiwindup
    if (pPI->antiwindPI_toggle == SET)
	{
      //Saturation
      pPI->Antiwindup_Term_s32 = ((pPI->PIout_sat_s32 - pPI->PIout_s32) * pPI->Antiwindup_Gain_s32);
    }
    else
	{
      pPI->Antiwindup_Term_s32 = 0;
    }
    //End Check Antiwindup    
  }
  else
  {
    pPI->PIout_sat_s32 = (pPI->PIout_s32);  
    pPI->Antiwindup_Term_s32 = 0;
  }
  //End Check Saturation
  pPI->PIout_sat_s32 = (pPI->PIout_sat_s32 >> pPI->PI_Q_FxP); 
  
  return pPI->PIout_sat_s32;  
}


/**
* @brief  Reset PI value
* @param  pPI: pointer to a DPC_PI_t  that contains
*         the configuration information and data for the specified PI. 
*
* @retval None
*
* @note Function valid for STM32G4xx microcontroller family  
*/ 
void PI_RESET(DPC_PI_t *pPI)
{
  pPI->Integral = pPI->resetValue;
}

// PBo : To Be Confirmed it is still making sens to keep this function as it is, while not used anymore
#if 0
/**
 * @brief     PID controller.
 * @details
 * This executes a PID controller with anti-windup.
 * 
 * Note that only the integral and proportional factors are actually
 * used with the default K* values. The following code is also 
 * an example of how digital control enables multiple calculations,
 * such as the antiwindup effect for non-linearly saturated systems. 
 *
 * @param     Ptr: pointer to a CNTRL_3p3zDataFloat that contains 
 *            the configuration information and data for the specified PID. 
 * @retval    None
 */
void CNTRL_pid( CNTRL_3p3zDataFloat* Ptr )
{
    // Update error vector
    Ptr->m_E[1] = Ptr->m_E[0];
    Ptr->m_E[0] = (Ptr->m_Ref - Ptr->m_Fdbk);
    err = Ptr->m_E[0];
    d_err = (Ptr->m_E[0] - Ptr->m_E[1]);

    // Integrate error with anti-windup
    pid_int_term += (pid_Ki * (err - prev_pid_antiw_term));

    // PID computation
    pid_output = (pid_const_term + (pid_Kp * err) + (pid_Kd * d_err) + pid_int_term);

    // Actuator Saturation
    pid_output_sat = MIN( pid_output , Ptr->m_Max );
    pid_output_sat = MAX( pid_output , -Ptr->m_Max );
    if (pid_output_sat < Ptr->m_Min)
	{
      pid_output_sat = Ptr->m_Min;
	}

    //Compute anti-windup for next PID execution
    prev_pid_antiw_term = ((pid_output - pid_output_sat) * pid_Ks);

    // Return value
    Ptr->m_Out = (int32_t)pid_output_sat;
}
#endif

/**
 * @brief     PID controller.
 * @details
 * This executes a PID controller with anti-windup.
 * 
 * Note that only the integral and proportional factors are actually
 * used with the default K* values. The following code is also 
 * an example of how digital control enables multiple calculations,
 * such as the antiwindup effect for non-linearly saturated systems. 
 *
 * @param     Ptr: pointer to a CNTRL_3p3zDataFloat that contains 
 *            the configuration information and data for the specified PID. 
 * @retval    None
 */

void CNTRL_PI_Regulator( CNTRL_PID_DataFloat* Ptr )
{
  // Update error vector
    float error = (Ptr->m_Ref - Ptr->m_Fdbk);
 
    
    float previous_integral_term = Ptr->m_Integral;  // for freezing in case 
    Ptr->m_Integral += Ptr->m_Ki * error;

    // PID computation
//    pid_output = (pid_const_term + (pid_Kp * err) + (pid_Kd * d_err) + pid_int_term);
    pid_output = Ptr->m_Kp * error + Ptr->m_Integral;

    // Actuator Saturation
    pid_output_sat = pid_output;

    if (pid_output_sat < Ptr->m_Min)
	{
        pid_output_sat = Ptr->m_Min;
        
        if (previous_integral_term > Ptr->m_Integral) // new integral even lower than previous one?
          Ptr->m_Integral -= Ptr->m_Ki * error;// freeze integral term
	}
    else if (pid_output_sat > Ptr->m_Max)
	{
        pid_output_sat = Ptr->m_Max;
        
        if (previous_integral_term < Ptr->m_Integral) // new integral even higher than previous one?
          Ptr->m_Integral -= Ptr->m_Ki * error;// freeze integral term
	}

    //Compute anti-windup for next PID execution
  //  prev_pid_antiw_term = ((pid_output - pid_output_sat) * pid_Ks);

    // Return value
    Ptr->m_Out = (int32_t)pid_output_sat;
}
